#include <string>
#include <cerrno>
#include "SignalDispatcher.h"

namespace uav
{
namespace comm
{

void SignalDispatcher::attachHandler(int sigNum, SignalHandler &handler)
		throw(SignalDispatcher::CannotAttachHandler)
{
	struct sigaction action;
	action.sa_handler = SignalDispatcher::actionHandler;
	sigemptyset(&action.sa_mask);
	action.sa_flags = 0;

	struct sigaction oldAction;
	if (sigaction(sigNum, &action, &oldAction) < 0)
		throw CannotAttachHandler(strerror(errno));

	if (SignalDispatcher::actionHandler != oldAction.sa_handler)
		oldActionList.insert(SigActionList::value_type(sigNum, oldAction));

	handlerList.insert(HandlerList::value_type(sigNum, handler));
}

void SignalDispatcher::detachHandler(int sigNum, const SignalHandler &handler)
		throw(SignalDispatcher::CannotDetachHandler, std::logic_error)
{
	std::pair<HandlerList::iterator, HandlerList::iterator> range = handlerList.equal_range(sigNum);

	// Find the specified handler in the list of handlers
	HandlerList::iterator handlerLocation = handlerList.end();
	for (HandlerList::iterator i = range.first; i != range.second; i++) {
		if (i->second == &handler) {
			handlerLocation = i;
			break;
		}
	}

	// If it is found, remove it
	if (handlerLocation != handlerList.end()) {
		handlerList.erase(handlerLocation);

		// If there are no more handlers for this signal, stop handling the
		// signal and restore the original signal action
		if (handlerList.count(sigNum) == 0) {
			SigActionList::iterator original = oldActionList.find(sigNum);
			if (original == oldActionList.end())
				throw std::logic_error("Signal dispatcher in invalid state.");
			if (sigaction(sigNum, &original->second, NULL) < 0)
				throw CannotDetachHandler(strerror(errno));
		}
	}
}

static void SignalDispatcher::actionHandler(int sigNum)
{
	// Get a list of handlers associated with this signal
	std::pair<HandlerList::iterator, HandlerList::iterator> range = handlerList.equal_range(sigNum);

	// Call each handler
	for (HandlerList::iterator i = range.first; i != range.second; i++) {
		i->second->handleSignal(sigNum);
	}
}

}} // end namespace
